#include <rclcpp/rclcpp.hpp>
#include <libbase/common.h>
#include "base_interfaces/msg/aimer.hpp"
#include "base_interfaces/msg/bt_aimer.hpp"
#include "base_interfaces/msg/scan_ctrl_info.hpp"

namespace wmj{
    class Mode : public rclcpp::Node
    {
    public:
        Mode(std::string name): Node(name)
        {
            aimer_sub = this->create_subscription<base_interfaces::msg::Aimer>("/Aimer", 2, std::bind(&Mode::aimer_cb, this, std::placeholders::_1));
            bt_aimer_pub = this->create_publisher<base_interfaces::msg::BtAimer>("BT_shooter", 1);
            scan_pub = this->create_publisher<base_interfaces::msg::ScanCtrlInfo>("ScanCtrlInfo", 1);
        }
    private:
        rclcpp::Subscription<base_interfaces::msg::Aimer>::SharedPtr aimer_sub;
        rclcpp::Publisher<base_interfaces::msg::BtAimer>::SharedPtr bt_aimer_pub;
        rclcpp::Publisher<base_interfaces::msg::ScanCtrlInfo>::SharedPtr scan_pub;
        void aimer_cb(const base_interfaces::msg::Aimer &msg)
        {
            if (msg.is_track)
            {
                base_interfaces::msg::BtAimer bt_aimer_msg;
                bt_aimer_msg.bullet = 1;
                bt_aimer_pub->publish(bt_aimer_msg);
            }
            else
            {
                base_interfaces::msg::ScanCtrlInfo scan_info;
                scan_info.scan_mode = 1;
                scan_pub->publish(scan_info);
            }
        }
    };
} //namespace wmj

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<wmj::Mode>("mode");
  /* 运行节点，并检测状态*/
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}